#ifndef _FORMATION_H
#define _FORMATION_H

#include <ewok/ed_ckecking_ring_buffer.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <mavros_msgs/State.h> 
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <cstdint>
#include <trajectory_msgs/JointTrajectory.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/filters/passthrough.h>
#include <nav_msgs/Path.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <algorithm>
#include <functional>
// #include "formation_avoidance/distribute_optimizer.h"
#include "formation_avoidance/utils.h"
#include "mavros_msgs/State.h"

#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708
#define AGENT_NUM 4
using namespace std;
class GridMapNode;
typedef GridMapNode *GridMapNodePtr;


class GridCube
{
public:
	GridCube(){}
	~GridCube(){}

	//Eigen::Vector3d p1, p2, p3, p4, p5, p6, p7, p8;   // the 8 vertex of a cube 
	Eigen::MatrixXf vertex_pos;
	Eigen::Vector3i center_idx; // the center of the cube
	bool valid;    // indicates whether this cube should be deleted
	Eigen::MatrixXi vertex_idx;

	double t; // time allocated to this cube
	std::vector< std::pair<double, double> > box;

	// create a cube using 8 vertex and the center point

	GridCube(Eigen::Vector3i node_index)
	{
		vertex_idx = Eigen::MatrixXi::Zero(8, 3);
		for(int i=0;i<8;i++)
		{
			vertex_idx.row(i)=node_index;
		}
		vertex_pos= Eigen::MatrixXf::Zero(8, 3);
		center_idx=node_index;
	}

	bool InflateCube(Eigen::MatrixXi last_InflateCube);
	bool VertexIsContained(Eigen::MatrixXi last_InflateCube);
	void UpdateVertexPos();


};


class GridMapNode
{
public:
	enum node_state
	{
		OPENSET = 1,
		CLOSEDSET = 2,
		UNDEFINED = 3
	};
	enum node_state state;
	GridMapNode(){}
	GridMapNode(Eigen::Vector3i node_index):index(node_index),g_score(0),h_score(0),f_score(0),state(UNDEFINED){}
	~GridMapNode(){}

	double g_score;
	double h_score;
	double f_score;
	Eigen::Vector3i index;
	int rounds{0};

    GridMapNode* parent_node;

	bool checkOccupancy();

	bool NodeKnown();
	double getdistance();
	double NodeDanger();

	std::vector<GridMapNode> getbresenham3D(GridMapNode* bresenham3Dend_ptr);
	int tmp;

};

class NodeComparator
{
public:
	bool operator()(GridMapNode* node1, GridMapNode* node2)
	{
		return node1->f_score > node2->f_score;
	}
};

class Formation_Agent
{

    enum FSM_EXEC_STATE
    {
      INIT,
      WAIT_TARGET,
      GEN_NEW_TRAJ,
      REPLAN_TRAJ,
      EXEC_TRAJ,
      EMERGENCY_STOP,
      SEQUENTIAL_START,
	  END_TRAJ
    };


public:
	Formation_Agent(int id):AGENT_ID(id),COLOR_ID(id+32),AgentInitFlag(false)
	{
		// ROS_INFO("[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[");
	}
	~Formation_Agent(){}

	int AGENT_ID;
	int COLOR_ID;
	ros::Subscriber cloud_sub;
	ros::Subscriber pose_sub;
    ros::Subscriber decision_velocity_sub;

	Eigen::Vector3d current_p[AGENT_NUM];
	Eigen::Quaterniond current_q[AGENT_NUM];
	std::queue<geometry_msgs::PoseStamped> pose_queue;
	double motor_yaw;
	double init_head_yaw;
	std::vector<geometry_msgs::PoseStamped> motor_yaw_queue;
	double CLOUD_DELAY;

	double D435_T265_OFFSET_X; 
	double D435_T265_OFFSET_Z; 

	double D435_BASE_OFFSET_X;
	double D435_BASE_OFFSET_Z;

	double T265_OFFSET_X;
	double T265_OFFSET_Y;
	double T265_OFFSET_Z;

    double decision_velocity_v;

	double CLIP_DISTANCE;

	bool map_initialized;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clip;

	ros::Publisher cloud_visual_pub2;
	ros::Publisher current_goal_position_visual_pub;
	ros::Publisher _corridor_vis_pub;

	ros::Publisher optimize_traj_visual_pub;

	std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> openSet_;
	std::vector<GridMapNode*> a_star_gridpath;

	const double tie_breaker_ = 4+ 1.0 / 10000;
	ros::Publisher a_star_pub;

    ros::Timer frontplantimer ;
    ros::Timer exec_timer_ ;
	ros::Timer sendpva_timer;

    ros::Publisher optimize_traj_pub ;
    ros::Publisher plan_head_pub ;
	ros::Subscriber gimbal_angle_sub;
	ros::Publisher pva_pub;

	ros::Subscriber state_sub;
	ros::Subscriber global_goal_sub;

	void init(ros::NodeHandle &nh);
	void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud);
	void pose_store(int index_temp,const geometry_msgs::PoseStampedPtr &msg );
	void poseCallback(const geometry_msgs::PoseStampedPtr &msg);
	void decision_velocity_Callback(const geometry_msgs::PointStampedPtr &msg);
	void map_visualization();
	void a_star_RemoveCollinearPath();


	double getDiagHeu(GridMapNode* node1, GridMapNode* node2);
	inline double getHeu(GridMapNode* node1,GridMapNode* node2)
	{
		return tie_breaker_ * getDiagHeu(node1, node2);
	}
	bool a_star_search(Eigen::Vector3f start_p,Eigen::Vector3f end_p);
	void a_star_retrievePath(GridMapNode* final_node);
	void visual_a_star_path();
	double getpointdistance(GridMapNode* node1, GridMapNode* node2);
	bool corridor_generation();
	void visual_corridor();
	void a_star_bresenham3D();
	void initGridMap();

    void FrontPlanTimecb(const ros::TimerEvent &e);
	bool traj_optimization();

	void HeadPlanning();
	double GetIG(double theta,Eigen::Vector3f observe_p);
	double getPathIG(std::vector<GridMapNode> raycastpath);

	double GetCG(double theta1,double theta2,Eigen::Vector3f observe_p1,Eigen::Vector3f observe_p2);
	bool checkInsideFov(std::vector<Eigen::Vector3f> normal_line,Eigen::Vector3f point,double theta,Eigen::Vector3f observe_p);
	double getPathCG(std::vector<Eigen::Vector3f> normal_line,std::vector<GridMapNode> raycastpath,double theta,Eigen::Vector3f observe_p,bool& SingalPathInsideFlag);
	void DistributeOptimizingHeadAngle();
    // double getHeadCost(Eigen::Vector4f headvector,bool base_value);
	double getHeadCost(Eigen::Vector4f headvector,bool base_value,double compare);
	void execFSMCallback(const ros::TimerEvent &e);
	void changeState(FSM_EXEC_STATE next_state);

	void sendPVA(const ros::TimerEvent &e);


	void gimbal_yaw_angleCallback(geometry_msgs::PoseStamped msg);
	void angle_mapping(double& angle);


	void uavmodecallback( const mavros_msgs::State &msg);
	void GlobalGoalCallback(const geometry_msgs::PoseStampedConstPtr &msg);

	int rounds_{0};
	int pool_size{110};
	int pool_size_z{10};

	double SAFETY_DISTANCE{0.3};
	std::vector<GridCube> CorridorCube;


	GridMapNodePtr ***GridMap_;
	Eigen::Vector3f plan_end_p;
	double PlanFreq{1.0};

	double IGSector[100][2];
	double hfov{1.047};

	double SectorAngle{hfov/4.0};

	double MaxCameraDis{5};

    double hmax=MaxCameraDis*tan(hfov/2)*2;
    double vmax=hmax/640.0*480.0;

	Eigen::Vector4f HeadAngleVector;

	int distribute_flag;
    Eigen::Vector3f headcost_p[4];

	std::vector<Eigen::Vector3f> planhead_p[4];

	double K_IG=-0.1;
	double K_CG=1.0;
	double K_IDEAL=0.5;

	double rate{30.0};

    std::vector<Eigen::Vector3f> OptimizedTraj;

    FSM_EXEC_STATE exec_state_;
	Eigen::Vector3f local_goal;
	Eigen::Vector3f global_goal;

    visualization_msgs::Marker traj_points;
	std::vector< std::pair<int,Eigen::Vector4f>> planHeadAngleKnotVector;
	double v=0.7;
public:
	bool AgentInitFlag;

	double local_length{1.8};
	float LAND_HEIGHT = 1.5; 
	
    trajectory_msgs::JointTrajectory traj;

	int sendpva_count=0;

	bool offboard_ready=false;

	bool a_star_flag=false;


	Eigen::Vector3f stop_p;

	bool first_fail=true;


	ros::Time plan_start_time;

	bool at_end{false};

	bool rec_global_goal_flag = false;

	double replan_time=1.0;

	GridMapNode tmp_node1;
	GridMapNode tmp_node2;

	bool at_goal=false;

	int sendpva_count_start{0};

};











#endif